/*
 *  updateTrajectory.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2007-11-28.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "updateTrajectory.h"
#include "math.h"
#include <OpenCv/OpenCV.h>
#include <iostream>


void PrintMat( CvMat *);

void circleChase( int nthLoop, double ballSpeed, CvMat **integrationErr, CvMat **setPoint, CvMat *tableCenter )
{
	ballSpeed=150;
	double pathRadius=60;
	double tau=(double)nthLoop/ballSpeed;
	cvZero( *setPoint ); 
	cvZero( *integrationErr );
	cvmSet( *setPoint, 0, 0, pathRadius*cos(-tau*2*pi)+cvmGet( tableCenter, 0, 0 ) ); 
	cvmSet( *setPoint, 2, 0, pathRadius*sin(-tau*2*pi)+cvmGet( tableCenter, 0, 1 ) ); 
	return;
}

void spiralChase( int nthLoop, double ballSpeed, CvMat **integrationErr, CvMat **setPoint, CvMat *tableCenter )
{
	ballSpeed=150;
	double pathMaxRadius=60;
	double tau=(double)nthLoop/ballSpeed;
	double pathRadius=(0.5+0.5*sin(tau*2*pi/5))*pathMaxRadius;
	cvZero( *setPoint ); 
	cvZero( *integrationErr );
	cvmSet( *setPoint, 0, 0, pathRadius*cos(-tau*2*pi)+cvmGet( tableCenter, 0, 0 ) ); 
	cvmSet( *setPoint, 2, 0, pathRadius*sin(-tau*2*pi)+cvmGet( tableCenter, 0, 1 ) ); 		
	return;
}

void ellipseChase( int nthLoop, double ballSpeed, CvMat **integrationErr, CvMat **setPoint, CvMat *tableCenter )
{
	ballSpeed=150;
	double pathRadius=70;
	double tau=(double)nthLoop/ballSpeed;
	cvZero( *setPoint ); 
	cvZero( *integrationErr );
	cvmSet( *setPoint, 0, 0, pathRadius*cos(-tau*2*pi)+cvmGet( tableCenter, 0, 0 ) ); 
	cvmSet( *setPoint, 2, 0, 0.5*pathRadius*sin(-tau*2*pi)+cvmGet( tableCenter, 0, 1 ) ); 		
	return;
}

void figure_eightChase( int nthLoop, double ballSpeed, CvMat **integrationErr, CvMat **setPoint, CvMat *tableCenter )
{
	ballSpeed=200;
	double pathRadius=50;
	double tau=(double)nthLoop/ballSpeed;
	bool onoff=( ((nthLoop) % (2*(int)ballSpeed)) < ballSpeed );
	cvZero( *setPoint ); 
	cvZero( *integrationErr );
	if (onoff){
		cvmSet( *setPoint, 0, 0, pathRadius*sin(tau*2*pi)+cvmGet( tableCenter, 0, 0 ) );
		cvmSet( *setPoint, 2, 0, -pathRadius*cos(tau*2*pi)+cvmGet( tableCenter, 0, 1 )+pathRadius);
	}
	else{
		cvmSet( *setPoint, 0, 0, pathRadius*sin(tau*2*pi)+cvmGet( tableCenter, 0, 0 ) );
		cvmSet( *setPoint, 2, 0, pathRadius*cos(tau*2*pi)+cvmGet( tableCenter, 0, 1 )-pathRadius);
	}
	return;
}

void stabilization( int nthLoop, CvMat **setPoint, CvMat *tableCenter )
{
	cvZero( *setPoint ); 
	printf("Got this far2\n");
	cvmSet( *setPoint, 0, 0, cvmGet( tableCenter, 0, 0 ) ); 
	cvmSet( *setPoint, 2, 0, cvmGet( tableCenter, 0, 1 ) );	
	return;
}


void updateTrajectory( char * pathName, int nthLoop, double ballSpeed, CvMat **integrationErr, CvMat **setPoint, CvMat *tableCenter )
// Should have some normalization wrt the sampling rate.
{
	if( strcmp(pathName,"circleChase") == 0 )
		circleChase( nthLoop, ballSpeed, integrationErr, setPoint, tableCenter );
	else if(strcmp(pathName,"spiralChase") == 0 )
		spiralChase( nthLoop, ballSpeed, integrationErr, setPoint, tableCenter );
	else if(strcmp(pathName,"ellipseChase") == 0)
		ellipseChase( nthLoop, ballSpeed, integrationErr, setPoint, tableCenter );
	else if(strcmp(pathName,"figure-eightChase") == 0)
		figure_eightChase( nthLoop, ballSpeed, integrationErr, setPoint, tableCenter );
	else
		stabilization( nthLoop, setPoint, tableCenter );
	return;
}